#include "global.h"
#include "mpu6050_reg.h"

#define CALIBRATION_SAMPLES 1000 // 校准时采样的次数

//+-2g 16384.0f +-4g 8192.0f +-8g 4096.0f +-16g 2048.0f
#define MPU6050_ACC_FS 8192.0f
//+-250dps 131.0f +-500dps 65.5f +-1000dps 32.8f +-2000dps 16.4f
#define MPU6050_GYRO_FS 16.4f

static _st_KalmanFilter kf[3];
static _st_LowPassFilter lpf[3];

uint8_t mpu6050_writeReg(uint8_t regAddress, uint8_t data)
{
    i2c_start();                   // I2C起始
    i2c_sendByte(MPU6050_ADDRESS); // 发送从机地址，读写位为0，表示即将写入
    if (i2c_receiveAck())          // 接收应答
        return ERROR;
    i2c_sendByte(regAddress); // 发送寄存器地址
    if (i2c_receiveAck())     // 接收应答
        return ERROR;
    i2c_sendByte(data);   // 发送要写入寄存器的数据
    if (i2c_receiveAck()) // 接收应答
        return ERROR;
    i2c_stop(); // I2C终止
    return SUCCESS;
}

uint8_t mpu6050_readReg(uint8_t regAddress, uint8_t *pData)
{
    i2c_start();                   // I2C起始
    i2c_sendByte(MPU6050_ADDRESS); // 发送从机地址，读写位为0，表示即将写入
    if (i2c_receiveAck())          // 接收应答
        return ERROR;
    i2c_sendByte(regAddress); // 发送寄存器地址
    if (i2c_receiveAck())     // 接收应答
        return ERROR;
    i2c_start();                          // I2C重复起始
    i2c_sendByte(MPU6050_ADDRESS | 0x01); // 发送从机地址，读写位为1，表示即将读取
    if (i2c_receiveAck())                 // 接收应答
        return ERROR;
    *pData = i2c_receiveByte(); // 接收指定寄存器的数据
    i2c_sendAck(0);             // 发送非应答，终止从机的数据输出
    i2c_stop();                 // I2C终止
    return SUCCESS;
}

uint8_t mpu6050_readRegs(uint8_t regAddress, uint8_t *pData, uint8_t length)
{
    i2c_start();                   // I2C起始
    i2c_sendByte(MPU6050_ADDRESS); // 发送从机地址，读写位为0，表示即将写入
    if (i2c_receiveAck())          // 接收应答
        return ERROR;
    i2c_sendByte(regAddress); // 发送寄存器地址
    if (i2c_receiveAck())     // 接收应答
        return ERROR;
    i2c_start();                          // I2C重复起始
    i2c_sendByte(MPU6050_ADDRESS | 0x01); // 发送从机地址，读写位为1，表示即将读取
    if (i2c_receiveAck())                 // 接收应答
        return ERROR;
    for (uint8_t i = 0; i < length; i++)
    {
        if (i == length - 1)
        {
            pData[i] = i2c_receiveByte(); // 接收最后一个字节的数据
            i2c_sendAck(0);               // 发送非应答，终止从机的数据输出
        }
        else
        {
            pData[i] = i2c_receiveByte(); // 接收指定寄存器的数据
            i2c_sendAck(1);               // 发送应答，继续读取
        }
    }
    i2c_stop(); // I2C终止
    return SUCCESS;
}

void mpu6050_init(void)
{
    uint8_t who_am_i;

    while (1)
    {
        // 1. 复位设备（必须执行且等待复位完成）
        if (mpu6050_writeReg(MPU6050_PWR_MGMT_1, 0x80) != SUCCESS)
        {
            delay_ms(10); // 防止频繁写寄存器导致总线锁死
            continue;     // 立即重试复位操作
        }
        delay_ms(150); // 等待复位完成

        // 2. 唤醒设备并设置时钟源
        if (mpu6050_writeReg(MPU6050_PWR_MGMT_1, 0x00) != SUCCESS)
        {
            delay_ms(10);
            continue; // 唤醒失败，重新开始整个流程
        }

        // 3. 验证 WHO_AM_I 寄存器
        if (mpu6050_readReg(MPU6050_WHO_AM_I, &who_am_i) != SUCCESS)
        {
            delay_ms(10);
            continue; // 读取失败，重新开始
        }
        if (who_am_i != 0x68 && who_am_i != 0x69)
        {
            delay_ms(10);
            continue; // 值非法，重新开始
        }

        // 4. 配置传感器参数（所有配置必须成功）
        if (mpu6050_writeReg(MPU6050_SMPLRT_DIV, 0x04) != SUCCESS) // 采样率 = 1kHz / (1+4) = 200Hz
        {
            delay_ms(10);
            continue;
        }
        if (mpu6050_writeReg(MPU6050_CONFIG, 0x03) != SUCCESS) // DLPF 带宽 44Hz
        {
            delay_ms(10);
            continue;
        }
        if (mpu6050_writeReg(MPU6050_GYRO_CONFIG, 0x18) != SUCCESS) // 陀螺仪 ±2000°/s
        {
            delay_ms(10);
            continue;
        }
        if (mpu6050_writeReg(MPU6050_ACCEL_CONFIG, 0x08) != SUCCESS) // 加速度计 ±4g
        {
            delay_ms(10);
            continue;
        }

        // 5. 所有步骤成功，退出初始化
        break;
    }
}

uint8_t mpu6050_getData(void)
{
    uint8_t data[14]; // 定义数据缓冲区，用于存储14个字节的数据

    // 读取14个连续寄存器的数据
    if (mpu6050_readRegs(MPU6050_ACCEL_XOUT_H, data, 14) != SUCCESS)
        return ERROR;

    // 解析数据
    mpu6050.accX = (data[0] << 8) | data[1];
    mpu6050.accY = (data[2] << 8) | data[3];
    mpu6050.accZ = (data[4] << 8) | data[5];
    mpu6050.temp = (data[6] << 8) | data[7];
    mpu6050.gyroX = (data[8] << 8) | data[9];
    mpu6050.gyroY = (data[10] << 8) | data[11];
    mpu6050.gyroZ = (data[12] << 8) | data[13];
    return SUCCESS;
}

void mpu6050_applyBias(void)
{
    mpu6050.accX -= mpu6050.accBiasX;
    mpu6050.accY -= mpu6050.accBiasY;
    mpu6050.accZ -= mpu6050.accBiasZ;
    mpu6050.gyroX -= mpu6050.gyroBiasX;
    mpu6050.gyroY -= mpu6050.gyroBiasY;
    mpu6050.gyroZ -= mpu6050.gyroBiasZ;
}

void mpu6050_getData_F(void)
{
    mpu6050_F.accX = mpu6050.accX / MPU6050_ACC_FS;
    mpu6050_F.accY = mpu6050.accY / MPU6050_ACC_FS;
    mpu6050_F.accZ = mpu6050.accZ / MPU6050_ACC_FS;
    mpu6050_F.temp = mpu6050.temp / 340.0f + 36.53f; // 温度计算公式
    mpu6050_F.gyroX = mpu6050.gyroX / MPU6050_GYRO_FS;
    mpu6050_F.gyroY = mpu6050.gyroY / MPU6050_GYRO_FS;
    mpu6050_F.gyroZ = mpu6050.gyroZ / MPU6050_GYRO_FS;
}

void mpu6050_fliterInit(void)
{
    for (uint8_t i = 0; i < 3; i++)
    {
        kalman_init(&kf[i], 0.0f, 0.02f, 0.001f, 0.543f); // 初始化卡尔曼滤波器
        lowPass_init(&lpf[i], 0.15f);                     // 初始化低通滤波器
    }
}

void mpu6050_fliterUpdate(void)
{
    kalman_predict(&kf[0]);                     // 卡尔曼滤波预测步骤
    kalman_update(&kf[0], (float)mpu6050.accX); // 一维卡尔曼
    mpu6050.accX = (int16_t)kf[0].x;

    kalman_predict(&kf[1]);                     // 卡尔曼滤波预测步骤
    kalman_update(&kf[1], (float)mpu6050.accY); // 一维卡尔曼
    mpu6050.accY = (int16_t)kf[1].x;

    kalman_predict(&kf[2]);                     // 卡尔曼滤波预测步骤
    kalman_update(&kf[2], (float)mpu6050.accZ); // 一维卡尔曼
    mpu6050.accZ = (int16_t)kf[2].x;

    lowPass_update(&lpf[0], (float)mpu6050.gyroX); // 一维低通滤波
    mpu6050.gyroX = (int16_t)lpf[0].x;

    lowPass_update(&lpf[1], (float)mpu6050.gyroY); // 一维低通滤波
    mpu6050.gyroY = (int16_t)lpf[1].x;

    lowPass_update(&lpf[2], (float)mpu6050.gyroZ); // 一维低通滤波
    mpu6050.gyroZ = (int16_t)lpf[2].x;
}

void mpu6050_setBias(int16_t accBiasX, int16_t accBiasY, int16_t accBiasZ,
                     int16_t gyroBiasX, int16_t gyroBiasY, int16_t gyroBiasZ) // 设置零偏
{
    mpu6050.accBiasX = accBiasX;
    mpu6050.accBiasY = accBiasY;
    mpu6050.accBiasZ = accBiasZ;
    mpu6050.gyroBiasX = gyroBiasX;
    mpu6050.gyroBiasY = gyroBiasY;
    mpu6050.gyroBiasZ = gyroBiasZ;
}

uint8_t mpu6050_calibrate() // 校准函数，静态采样并计算零偏
{
    int32_t accSumX = 0, accSumY = 0, accSumZ = 0;
    int32_t gyroSumX = 0, gyroSumY = 0, gyroSumZ = 0;
    int32_t tempSum = 0;

    // 采集多次数据以计算平均零偏
    for (int i = 0; i < CALIBRATION_SAMPLES; i++)
    {
        if (mpu6050_getData() != SUCCESS) // 获取数据
            return ERROR;

        // 累加数据
        accSumX += mpu6050.accX;
        accSumY += mpu6050.accY;
        accSumZ += mpu6050.accZ;
        tempSum += mpu6050.temp;
        gyroSumX += mpu6050.gyroX;
        gyroSumY += mpu6050.gyroY;
        gyroSumZ += mpu6050.gyroZ;

        delay_ms(10); // 延时10ms
    }

    // 计算平均值
    mpu6050.accBiasX = accSumX / CALIBRATION_SAMPLES;
    mpu6050.accBiasY = accSumY / CALIBRATION_SAMPLES;
    mpu6050.accBiasZ = accSumZ / CALIBRATION_SAMPLES - MPU6050_ACC_FS;
    mpu6050.temp = tempSum / CALIBRATION_SAMPLES;
    mpu6050.gyroBiasX = gyroSumX / CALIBRATION_SAMPLES;
    mpu6050.gyroBiasY = gyroSumY / CALIBRATION_SAMPLES;
    mpu6050.gyroBiasZ = gyroSumZ / CALIBRATION_SAMPLES;
    return SUCCESS;
}
